
#include "slip.h"

SlipStream::SlipStream(HardwareSerial &serial, size_t maxFrameSize)
    : serial(serial), maxFrameSize(maxFrameSize), escapePending(false), droppedFramesCount(0) {
    rxBuffer.reserve(maxFrameSize);
}

void SlipStream::send(const uint8_t *payload, size_t length) {
    serial.write(END);
    for (size_t i = 0; i < length; ++i) {
        uint8_t byte = payload[i];
        if (byte == END) {
            serial.write(ESC);
            serial.write(ESC_END);
        } else if (byte == ESC) {
            serial.write(ESC);
            serial.write(ESC_ESC);
        } else {
            serial.write(byte);
        }
    }
    serial.write(END);
}

int SlipStream::poll() {
    while (serial.available()) {
        uint8_t byte = serial.read();
        processByte(byte);
    }
    return rxFrames.size();
}

int SlipStream::feed(const uint8_t *data, size_t length) {
    int frameCount = 0;
    for (size_t i = 0; i < length; ++i) {
        frameCount += processByte(data[i]);
    }
    return frameCount;
}

bool SlipStream::has_frame() const {
    return !rxFrames.empty();
}

int SlipStream::dropped_frames() const {
    return droppedFramesCount;
}

void SlipStream::flush_rx() {
    rxFrames.clear();
    rxBuffer.clear();
    escapePending = false;
}

int SlipStream::recv(uint8_t *outBuffer, size_t &outLength) {
    if (rxFrames.empty()) {
        outLength = 0;
        return 0;
    }
    std::vector<uint8_t> &frame = rxFrames.front();
    outLength = frame.size();
    memcpy(outBuffer, frame.data(), outLength);
    rxFrames.erase(rxFrames.begin());
    return 1;
}

int SlipStream::processByte(uint8_t byte) {
    if (byte == END) {
        return finishFrame();
    }
    if (escapePending) {
        escapePending = false;
        if (byte == ESC_END) {
            return appendByte(END);
        } else if (byte == ESC_ESC) {
            return appendByte(ESC);
        } else {
            return dropFrame();
        }
    }
    if (byte == ESC) {
        escapePending = true;
        return 0;
    }
    return appendByte(byte);
}

int SlipStream::appendByte(uint8_t byte) {
    if (rxBuffer.size() >= maxFrameSize) {
        return dropFrame();
    }
    rxBuffer.push_back(byte);
    return 0;
}

int SlipStream::finishFrame() {
    if (rxBuffer.empty()) {
        return 0;
    }
    rxFrames.push_back(rxBuffer);
    rxBuffer.clear();
    escapePending = false;
    return 1;
}

int SlipStream::dropFrame() {
    rxBuffer.clear();
    escapePending = false;
    ++droppedFramesCount;
    return 0;
}
